/**
 * @file RuneTrack.cpp
 * @author enemy1205 (enemy1205@qq.com)
 * @brief 神符追踪类
 * @date 2021-09-12
 *
 */
#include "RuneTrack.h"
#include <opencv2/calib3d/calib3d.hpp>

using namespace cv;

//////倘若第一帧即错误识别(0,0)，之后全错，该方法暂弃用，换以增加构造时识别的筛选条件解决
void RuneTracker::getRealCenter(Rune_ptr &currentRune) {
    if (currentRune->getCenter() == cv::Point2f(0, 0))
        currentRune->getCenter() = this->features_sequence.front()->getCenter();//倘若(0,0)异常，用上帧能量机关中心替代
}

/**
 * @brief 由新神符构造神符追踪器
 * @param rune 新的神符
 */
RuneTracker::RuneTracker(Features_ptr &rune) {
    // 记录首帧装甲板
    if (rune == nullptr) {
        return;
    }
    this->features_sequence.emplace_front(rune);
    this->vanish_num = 0;
    this->KF_Rune = KalmanFilterX<2, 2>(kalmanFilterParam.ANGLE_PROCESS_ERROR, kalmanFilterParam.ANGLE_MEASURE_ERROR);
    this->KF_Rune.setH(Matx<float, 2, 2>::eye());
}

/**
 * @brief 更新神符时间序列
 * @param currentRune 当前帧神符
 */
void RuneTracker::update(const Features_ptr &currentRune) {
    if (currentRune != nullptr) {
        //修正中心
//        this->getRealCenter(currentRune);
        // 打入队列
        this->features_sequence.push_front(currentRune);
        // 重置消失帧数
        this->vanish_num = 0;
    } else {
        this->vanish_num++;
    }
    // 判断是否超容量
    if (this->features_sequence.size() > (size_t) R_param.RECORD_FRAMES) {
        this->features_sequence.pop_back();
    }
}

/**
 * @brief 为匹配判断是否发生较大位置突变
 * @param to_be_matched 待匹配进入序列的特征
 * @param delta 暂未用到参数
 * @return true表示位移过大，需新建序列
 */
bool RuneTracker::isSuddenChange(Features_ptr &to_be_matched, float &delta) {
    // 获取角度差
    float delta_angle = getDeltaAngle(to_be_matched->getAngle(), this->getGoalFeature()->getAngle());
    // 判断是否角度差过大
    if (delta_angle > R_param.MAX_DELTA_ANGLE) return true;
    else return false;
}
